/**
 * @convert2imr.cpp
 * @author Jianan Lou
 * @date 2021.6.3
 * @brief Function definitions of convert2imr. Other file formats can be converted to IMR format. 
 *
 *
 * @details
 * @see
 */

#include "convert2imr_backup.h"
using namespace std;
/*!
 *  @brief  Set the header of IMR file
 *
 *
 *  @param[in]   none
 *
 *  @param[out]  none
 *
 *  @return      void
 */
void IMRConverter_LJN::initIMRHeader()
{
	strcpy(_imr_header._header, "$IMURAW");
	_imr_header._is_intel_or_motorola = 0;           /* 0 = Intel (Little Endian), default */
	_imr_header._version_number = 8.80;         	 /* Default version number 8.80 */
	_imr_header._delta_theta = 0;	                 /* 0 = Data to follow will be read as scaled angular rates */
	_imr_header._delta_velocity = 0;	             /* 0 = Data to follow will be read as scaled accelerations */
	_imr_header._data_rate = 100;                    /* The data rate of the IMU in Hz. e.g. 0.01 second data rate is 100Hz */
	_imr_header._gyro_scale_factor = 0.0001;         /* multiply the gyro measurements by 0.0001 to get degrees/second */
	_imr_header._accel_scale_factor = 0.0001;        /* multiply the accel measurements by this to get m/s2 */
	_imr_header._utc_or_gpstime = 0;                 /* 0 = Unknown, will default to GPS */
	_imr_header._rcvtime_or_corrtime = 0;            /* 0 = Unknown, will default to corrected time */
	_imr_header._time_tag_bias = 0.0;                /* A known bias between your GPS and IMU time tags can be entered here, temporarily set to 0 */
	strcpy(_imr_header._imu_name, "INS-D");          /* Name of the IMU being used - INS-D */
	strcpy(_imr_header._program_name, "NavNewton");  /* Name of calling program - NavNewton */
	_imr_header._t_create = { 2021,5,27 };           /* Creation time of file, temporarily set to 2021,5,27 */
	_imr_header._leverarm_valid = false;             /* False because lever arms from IMU to primary GNSS antenna are not stored in this header */
	_imr_header._x_offset = 0;                       /* X value of the lever arm, in millimeters, temporarily set to 0 */ 
	_imr_header._y_offset = 0;						 /* Y value of the lever arm, in millimeters, temporarily set to 0 */ 
	_imr_header._z_offset = 0;						 /* Z value of the lever arm, in millimeters, temporarily set to 0 */ 
}


/*!
 *  @brief  Write the header of IMR file
 *
 *
 *  @param[in]  outfile   Pointer to the output file
 *
 *  @param[in]  none
 *
 *  @return     void
 */
void IMRConverter_LJN::writeIMRHeader(ofstream& outfile)
{
	outfile.write((char*)_imr_header._header, sizeof(_imr_header._header));
	outfile.write((char*)&_imr_header._is_intel_or_motorola, sizeof(_imr_header._is_intel_or_motorola));
	outfile.write((char*)&_imr_header._version_number, sizeof(_imr_header._version_number));
	outfile.write((char*)&_imr_header._delta_theta, sizeof(_imr_header._delta_theta));
	outfile.write((char*)&_imr_header._delta_velocity, sizeof(_imr_header._delta_velocity));
	outfile.write((char*)&_imr_header._data_rate, sizeof(_imr_header._data_rate));
	outfile.write((char*)&_imr_header._gyro_scale_factor, sizeof(_imr_header._gyro_scale_factor));
	outfile.write((char*)&_imr_header._accel_scale_factor, sizeof(_imr_header._accel_scale_factor));
	outfile.write((char*)&_imr_header._utc_or_gpstime, sizeof(_imr_header._utc_or_gpstime));
	outfile.write((char*)&_imr_header._rcvtime_or_corrtime, sizeof(_imr_header._rcvtime_or_corrtime));

	outfile.write((char*)&_imr_header._time_tag_bias, sizeof(_imr_header._time_tag_bias));
	outfile.write((char*)_imr_header._imu_name, sizeof(_imr_header._imu_name));
	outfile.write((char*)_imr_header._reserved1, sizeof(_imr_header._reserved1));
	outfile.write((char*)_imr_header._program_name, sizeof(_imr_header._program_name));
	outfile.write((char*)&_imr_header._t_create, sizeof(_imr_header._t_create));
	outfile.write((char*)&_imr_header._leverarm_valid, sizeof(_imr_header._leverarm_valid));
	outfile.write((char*)&_imr_header._x_offset, sizeof(_imr_header._x_offset));
	outfile.write((char*)&_imr_header._y_offset, sizeof(_imr_header._y_offset));
	outfile.write((char*)&_imr_header._z_offset, sizeof(_imr_header._z_offset));
	outfile.write((char*)_imr_header._reserved, sizeof(_imr_header._reserved));
}


/*!
 *  @brief  Conversion from IMU format to IMR format
 *
 *
 *  @param[in]  imu_filename	 The file name of IMU format data
 *
 *  @param[in]  imr_filename     The file name of IMR format data
 *
 *  @return     True/False       Conversion success is True, conversion failure is False
 */
bool IMRConverter_LJN::IMU2IMR(const char* imu_filename, const char* imr_filename)
{
	double time;       /*Time of the current measurement*/
	int32_t gyro[3];   /*Scaled gyro measurement about the IMU three axes*/
	int32_t accel[3];  /*Scaled accel measurement about the IMU three axes*/

	ifstream infile(imu_filename, ios::binary);  /*Open imu file in binary for reading*/
	ofstream outfile(imr_filename, ios::binary); /*Open imr file in binary for writing*/

	if (!infile.is_open() && !outfile.is_open()) /*Return false if the file is not opened successfully.*/
		return false;

	writeIMRHeader(outfile); /*Write the header of IMR file*/

	 /*The main part of data that is read and written at the same time*/
	while (!infile.eof())
	{
		infile.read((char*)&time, sizeof(time));
		infile.read((char*)gyro, sizeof(gyro));
		infile.read((char*)accel, sizeof(accel));

		outfile.write((char*)&time, sizeof(time));
		outfile.write((char*)gyro, sizeof(gyro));
		outfile.write((char*)accel, sizeof(accel));
	}
	infile.close();
	outfile.close();
	return true;
}
